/*
STM32部分只需要听从K210的命令，完美执行，完了之后及时反馈即可

todo list：
1.编码器测到的速度累加起来的位移 = 0.7649459121011438 * 实际位移。该公式需要继续测量
2.尽管加入了PID算法，试图让车子走直线，但做得还是不够好。应该修正一下各参数，力求达到更好的稳定性
3.精进角度阈值

记得每次测试前先校准速度比例，尽管加入了PID算法来使得其尽量走直线，但如果一开始比例就十分得当的话收敛到理想配比的速度会更快。测试功能的话可以给接收数据包
的数组Serial_RxPacket赋相应初值，也可以连上K210，顺便看它们的串口功能是否正常。
*/

#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "Serial.h"
#include "Key.h"
#include "Motor.h"
#include "MPU6050.h"
#include "Timer.h"
#include "Encoder.h"
#include "PID.h"

#define TARGET_ANGLE 84//因为每个旋转操作都固定要转90°，此处就不用变量来记录目标角度了，直接宏定义一个常数。在真正转过90°时累加出来的角度偏小，而且因为惯
//性的原因，无法及时制动（我看有些是通过让电机瞬间反转实现的，我怕一下变太多造成什么问题就没这样做），因此把阈值给小一点
#define DISPLACEMENT_CHANGING_RATE 0.7423888182973331//实际位移乘上该比例即可得到所需的、虚拟的目标位移
#define GZ_CORRECTION_COEFFICIENT 4//陀螺仪在静态情况下测量出的角速度在+1~+2之间波动，因此在使用该值时需要减去该修正系数

uint8_t count = 0;//记录已经接收到了几个数据包，在OLED屏幕上显示。纯粹用于调试K210中的command_table是否合理
uint8_t KeyNum;			//定义用于接收按键键码的变量
uint16_t Speed1 = 96;
uint16_t Speed2 = 100;//左、右两轮应达到的转速。这转速会在开始
int16_t Speed;//某个轮子的实际位移。因为车辆在前进时只会直走，所以只测量其中一个轮子的速度即可反映整辆车的速度，进而求得位移。
float displacement;//当前位移
float target_displacement;//目标位移。每隔0.05秒，车子的速度就会被检测一次，如果此时正处于前进状态，就会与目标位移进
//行比较，只要超过了就会停下。
float angle;//当前角度。越大代表越向左。
float target_angle;//目标角度。
int16_t GZ;			//定义绕Z轴的角速度。MPU6050所能测量的其余物理量暂时看来没有用处

int main(void)
{
	/*模块初始化*/
	OLED_Init();		//OLED初始化
	Key_Init();			//按键初始化
	Serial_Init();		//串口初始化
	MPU6050_Init();		//MPU6050初始化
	Timer_Init();		//定时器初始化
	Encoder1_Init();		//编码器初始化
	Motor_Init();		//直流电机初始化
	PID_init();//PID结构体初始化
	
	
	Serial_TxPacket[0] = 3;//设置发送数据包数组的初始值
	
	Serial_RxPacket[2] = 3;//先进入待机状态
	
//	Motor_SetSpeed_1(Speed1);
//	Motor_SetSpeed_4(Speed2);
	target_displacement = 0.0f;
	Serial_Printf("GET\r\n");
	while (1)
	{	
		KeyNum = Key_GetNum();			//获取按键键码
		if (KeyNum == 1)				//按键1按下
		{
			Serial_TxPacket[0] = 00;		
			Serial_SendPacket();		//串口发送数据包Serial_TxPacket
		}
	    Serial_TxPacket[0] = 3;
		Serial_TxPacket[1] = 3;  //发送完进入待机状态
		if (Serial_GetRxFlag() == 1)	//如果接收到数据包
		{
			count++;
//			OLED_ShowHexNum(4, 1, Serial_RxPacket[0], 2);	//显示接收的数据包
//			OLED_ShowHexNum(4, 4, Serial_RxPacket[1], 2);
//			OLED_ShowHexNum(4, 7, Serial_RxPacket[2], 2);
//			OLED_ShowHexNum(4, 10, Serial_RxPacket[3], 2);
//			Serial_Printf("GET\r\n");
			if(Serial_RxPacket[2] == 0)//移动
			{
				if(Serial_RxPacket[0] == 0)//前进
				{//如果在移动的过程中又有新的命令发来，那一定是K210检测到二维码了，此时应当以新的命令为准
					displacement = 0;
					
					Motor_SetSpeed_1(Speed1);
					Motor_SetSpeed_4(Speed2);
					target_displacement = (float)Serial_RxPacket[1] * DISPLACEMENT_CHANGING_RATE;
					OLED_ShowFNum(3, 1, target_displacement);
				}
				else if(Serial_RxPacket[0] == 1)//右转
				{
					Motor_SetSpeed_1(0);
					Motor_SetSpeed_4(Speed2);
					angle = 0;
					target_angle = (float)Serial_RxPacket[1];
				}
				else if(Serial_RxPacket[0] == 2)//左转
				{
					Motor_SetSpeed_1(Speed1);
					Motor_SetSpeed_4(0);
					angle = 0;
					target_angle = (float)Serial_RxPacket[1];
				}
			}
			else if(Serial_RxPacket[2] == 1)//货物数据。其余的都是无效命令
			{
				OLED_ShowHexNum(1, 1, Serial_RxPacket[0] * 16 + Serial_RxPacket[1], 2);
			}
		}
		if(Serial_RxPacket[0] == 0 && displacement > target_displacement)//走够了
		{
			Motor_SetSpeed_1(0);
			Motor_SetSpeed_4(0);
			displacement = 0;//测试时可以暂且不要清零实际位移，当车子停下后量一下实际距离，得到编码器计次和实际距离的映射关系
			target_displacement = 0;
			Serial_RxPacket[0] = 3;//不要在下一个循环里继续前进或旋转了
			Serial_SendPacket();//告诉K210当前步骤执行完了，应该继续下达下一个命令
		}
		if((Serial_RxPacket[0] == 1 && angle < -target_angle) || (Serial_RxPacket[0] == 2 && angle > target_angle))//转够了。
		{
			Motor_SetSpeed_1(0);
			Motor_SetSpeed_4(0);
			angle = 0;//这里也是一样的
			Serial_RxPacket[0] = 3;
			Serial_SendPacket();
		}
		OLED_ShowFNum(1, 1, displacement);	//当前位移
//		OLED_ShowFNum(2, 1, target_displacement);	//目标位移
		OLED_ShowFNum(2, 1, angle);	//当前角度
		OLED_ShowFNum(3, 1, target_angle);	//目标位移
//		OLED_ShowNum(3, 1, Speed1, 3);	//右轮速度
//		OLED_ShowNum(4, 1, Speed2, 3);	//左轮速度
		OLED_ShowNum(1, 14, count, 2);
 		MPU6050_GetData(&GZ);		//获取MPU6050的数据
		OLED_ShowSignedNum(4, 1, GZ, 5);	//角速度
		if(Serial_RxPacket[2] == 0 && Serial_RxPacket[0] == 0)//当且仅当正在直走的时候，通过PID算法不断修正两轮速度，使其恰好直走。转弯的时候当然就不用调整了
		{
			PID_patroling(GZ - GZ_CORRECTION_COEFFICIENT, &Speed1, &Speed2);
			Motor_SetSpeed_1(Speed1);
			Motor_SetSpeed_4(Speed2);
		}
	}
}

/**
  * 函    数：TIM2中断函数。小车在运动时每隔一段时间就要查看一次当前的速度，以便及时停下行进任务。分频值配置在Timer.h中，此处是0.05秒中断一次
  * 参    数：无
  * 返 回 值：无
  * 注意事项：此函数为中断函数，无需调用，中断触发后自动执行
  */
void TIM2_IRQHandler(void)
{
	if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)		//判断是否是TIM2的更新事件触发的中断
	{
		Speed = Encoder1_Get();
		//每隔固定时间段读取一次编码器计数增量值，即为速度值。认为两次中断的过程中速度并未发生变化，求得微小位移并累加。反正它不具有实际意义，所以把它除得小
		//了一些（真实计数值的十分之一），目标位移也不不用设置得太大
		displacement += (float)Speed / 100.0f / 2.0f;
		
		//对于角度也是一个道理。直走时它能辅助车子不偏航，转弯时更是要根据这个来判断是否转到位了。MPU6050满量程被设置为±2000°/s，对应AD值为-32768~32767，
		//即GZ除32768再乘2000即为以度为单位的角速度。在完全没有动时GZ测出来也为+1~+2，此处对它进行修正。
		angle += (((float)GZ) - GZ_CORRECTION_COEFFICIENT) / 32768.0f * 2000.0f / 10.0f / 2.0f;
		TIM_ClearITPendingBit(TIM2, TIM_IT_Update);			//清除TIM2更新事件的中断标志位
															//中断标志位必须清除
															//否则中断将连续不断地触发，导致主程序卡死
	}
}
